// #ifndef _PCLFUNCTIONS_
// #define _PCLFUNCTIONS_
#pragma once

// #include "lds_lidar.h"
#include "livox.h"
// #include "livox_m.h"
#include "Communication.h"

#include <unistd.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <iostream>
#include <opencv2/core/eigen.hpp>
#include <opencv2/opencv.hpp>
#include <string>
#include <vector>
// cylinder segmentation --> //
#include <pcl/ModelCoefficients.h>
#include <pcl/console/time.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>
#include <pcl/segmentation/sac_segmentation.h>
// <-- //

#include <pcl/features/integral_image_normal.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/mls.h>
#include <pcl/visualization/cloud_viewer.h>
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "odometry.h"

typedef pcl::PointXYZINormal PointTypeFull;
typedef pcl::PointCloud<PointTypeFull> Fcloud;
typedef Fcloud::Ptr Pfcloud;
// void Extract_All_Planes(const Pcloud InCloud, Pcloud OutCloud);
class PCLF
{
public:
    PCLF(const std::string cfgFile);
    void filter(const Pcloud& inputCloudPtr, Pcloud& outputCloudPtr);
    void filter(const PcloudI& inputCloudPtr, PcloudI& outputCloudPtr);
    void computeNormals(const Pcloud& inputCloudPtr, Pcnormal& outputNormalsPtr, const int num);
    void computeNormals(const PcloudI& inputCloudPtr, Pfcloud& outputNormalsPtr, const int num);

    void removePlanes(const Pcloud& inputCloudPtr, const Pcnormal& inputnormalsPtr, int num, bool inverted, Pcloud& outputCloudPtr, Pcnormal& outputNormalsPtr);
    void removePlanes(const PcloudI& inputCloudPtr, const Pcnormal& inputnormalsPtr, int num, bool inverted, PcloudI& outputCloudPtr,
                      Pcnormal& outputNormalsPtr);

    void eucluster(const Pcloud& inputCloudPtr, std::vector<pcl::PointIndices>& indices);
    void eucluster(const PcloudI& inputCloudPtr, std::vector<pcl::PointIndices>& indices);

    void extraction(const Pcloud& inputCloudPtr, const Pcnormal& inputNormalsPtr, const pcl::PointIndices::Ptr indicesPtr, Pcloud& outputCloudPtr,
                    Pcnormal& outputNormalsPtr, bool inverted = true);
    void extraction(const PcloudI& inputCloudPtr, const Pcnormal& inputNormalsPtr, const pcl::PointIndices::Ptr indicesPtr, PcloudI& outputCloudPtr,
                    Pcnormal& outputNormalsPtr, bool inverted = true);

    bool fitCylinder(const Pcloud& inputCloudPtr, const Pcnormal& inputNormalsPtr, Pcloud& outputCloudPtr, Pcnormal& outputNormalsPtr,
                     pcl::ModelCoefficients::Ptr& coePtr, Eigen::Vector2f& catchPoint, bool inverted);
    bool fitCylinder(const PcloudI& inputCloudPtr, const Pcnormal& inputNormalsPtr, PcloudI& outputCloudPtr, Pcnormal& outputNormalsPtr,
                     pcl::ModelCoefficients::Ptr& coePtr, Eigen::Vector2f& catchPoint, bool inverted);
    void axialDrawing(PcloudI& inputCloudPtr, const pcl::ModelCoefficients::Ptr& coePtr);
    void axialDrawing(Pcloud& inputCloudPtr, const pcl::ModelCoefficients::Ptr& coePtr);

    void conditionalEucluster(const Pfcloud& inputCloudPtr, pcl::IndicesClusters& indices);
    static bool intensitySimilarity(const PointTypeFull& point_a, const PointTypeFull& point_b, float radiu);
    static bool intensityNormalSimilarity(const PointTypeFull& point_a, const PointTypeFull& point_b, float radiu);
    float pileLagori(const PcloudI& inputClusterPtr, float threshHeight);
    void fusion(const Pcloud& inputCloudPtr, Pcloud& outputCloudPtr, const Eigen::Matrix4f& RoT = Eigen::Matrix4f::Identity());
    void fusion(const PcloudI& inputCloudPtr, PcloudI& outputCloudPtr, const Eigen::Matrix4f& RoT = Eigen::Matrix4f::Identity());
    void fusion(const CloudI& inputCloud, PcloudI& outputCloudPtr, const Eigen::Matrix4f& RoT = Eigen::Matrix4f::Identity());
    void fusion(const CloudI& inputCloud, cv::Mat& outputDepthImg, const Eigen::Matrix4f& cam2Lidar_RoT, const Eigen::Matrix3f& camIntrix,
                const Eigen::Matrix4f& RoT = Eigen::Matrix4f::Identity());
    Eigen::Matrix4f RoT(const Eigen::Matrix4f& inputRoT, const Eigen::Vector4f& inputTran);
    void Gydata2RoT(const Gydata inputGydata_Obj, const Gydata inputGydata_Des, Eigen::Matrix4f& RoT);
    int WhichId(const float radiu);

private:
    //圆柱拟合参数
    float m_NDWeight_cy;
    int m_maxIterations_cy;
    float m_distThreshold_cy;
    float m_radius1;
    float m_radius2;
    //欧式聚类参数
    float m_euThreshold;
    int m_minSize;
    int m_maxSize;
    //平面拟合参数
    float m_NDWeight_pl;
    int m_maxIterations_pl;
    int m_distThreshold_pl;
    //法线计算
    int m_ksearchNum;
    //点云滤波
    float m_xmin, m_xmax;
    float m_ymin, m_ymax;
    float m_zmin, m_zmax;
    float m_rsearch;
    int m_minNeighborNum;
};

// #endif